#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include "dog.h"

#include "kinematics.h"
#include "pca9685.h"

#include "ohos_init.h"
#include <cmsis_os2.h>


// left front leg
#define LF_A    4
#define LF_B    5
// left back leg
#define LB_A    10
#define LB_B    11
// right front leg
#define RF_A    3
#define RF_B    2
// right back leg
#define RB_A    13
#define RB_B    12


#define OFFSET_LF_A     45
#define OFFSET_LF_B     -45
#define OFFSET_LB_A     45
#define OFFSET_LB_B     -45

#define OFFSET_RF_A     45
#define OFFSET_RF_B     -45
#define OFFSET_RB_A     45
#define OFFSET_RB_B     -45


static int is_loop = 0;
static int mode = 0;

static void set_angle(int port, int offset, int angle) {
    pca9685_servo_set_angle(port, offset + angle);
}

static void do_step(void) {
    struct Point_t start = {8.0, 80.0};
    struct Point_t end = {20.0, 45.0};

    double *start_angle = inverse(start);
    double *end_angle = inverse(end);

    if (start_angle && end_angle) {
        printf("start: %f, %f\n", start_angle[0], start_angle[1]);
        printf("end: %f, %f\n", end_angle[0], end_angle[1]);

        // left front leg down
        set_angle(LF_A, OFFSET_LF_A, start_angle[0]);
        set_angle(LF_B, OFFSET_LF_B, start_angle[1]);

        usleep(200 * 1000);

        // left front leg up
        set_angle(LF_A, OFFSET_LF_A, end_angle[0]);
        set_angle(LF_B, OFFSET_LF_B, end_angle[1]);

        usleep(200 * 1000);

        free(start_angle);
        free(end_angle);
    } else {
        printf("step failed\n");
    }
}


static void loop(void) {
    while (is_loop) {

        if (mode == 1) {
            // step
            do_step();
        }

    }
}

static void begin(void) {
    if (is_loop) return;
    is_loop = 1;

    osThreadAttr_t attr;
    attr.name = "DOG_TASK";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 1024 * 4;
    attr.priority = 25;

    if (osThreadNew((osThreadFunc_t) loop, NULL, (const osThreadAttr_t *) &attr) == NULL) {
        printf("Falied to create DOG_MAIN_TASK!\r\n");
    }
}

void dog_config_install(void) {
    set_angle(LF_A, OFFSET_LF_A, 90);
    set_angle(LF_B, OFFSET_LF_B, 180);
    set_angle(LB_A, OFFSET_LB_A, 90);
    set_angle(LB_B, OFFSET_LB_B, 180);
}

void dog_step(void) {
    begin();
    mode = 1;

    printf("dog step\n");

}


